% Set serial port
serialPortId = '/dev/tty.usbserial-FTFO9L12';% '/dev/tty.usbserial-FTFO9HPY';
serialPort = serial(serialPortId, 'BaudRate', 9600, 'DataBits', 8, 'Timeout', 1.5);

anchorNum = 9;
AREA{1} = [1 2 4 5];
AREA{2} = [2 3 5 6];
AREA{3} = [4 5 7 8];
AREA{4} = [5 6 8 9];
% Open Serial Port
try
    fopen(serialPort);
catch ME
    error('failed to open serial port');
end


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   *   *   *
    %     1   2
    %   *   *   *
    %     3   4
    %   *   *   *  --> the deployment
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
while 1
    cmd = imput('get location? yes: 1; quit: 0');
    if cmd == 0
        break;
    end
    
    rxInfo = fscanf(serialPort)
    info = textscan(rxInfo, '%d,%d,%d,%d,%d,%d,%d,%d,%d')
    
    dist = 0;
    [~, tmp4Highest] = sort(info,'descend');
    tmp4Highest = tmp4Highest(1:4);
    maxInter = 0;
    maxInterCount = 0;
    for i = 1 : 4
        inter = intersect(tmp4Highest, AREA{i});
        if (size(inter,2) > maxInterCount)
            maxInterCount = size(inter,2);
            maxInter = i;
        end
    end
    printf('u r in AREA: ' + maxInter + '\r\n');
    drawGrids(maxInter);
    % show image of the area
end
    
% Close port
try
    stopasync(serialPort);
    fclose(serialPort);
    delete(serialPort);
    clear serialPort;
catch ME
    error('fail to close the serial port, check connection and name'); 
end